#include "camera_packet.hpp"

namespace gimabl {

    CameraPacket::CameraPacket(uint32_t dataLen)
    :m_header(0xffff),
    m_type(0xff),
    m_flag(0xff),
    m_srcId(0xffff),
    m_dstId(0xffff),
    m_msgId(0xffff),
    m_serial(0xff),
    m_payloadLen(0xff),
    m_payloadBuf(nullptr),
    m_crcCheck(0xffff),
    m_dataBuf(nullptr),
    m_dataLen(dataLen),
    m_haveData(false),
    m_haveAckData(false)
    {
        if(m_dataLen>0)
        {
            m_dataBuf=(uint8_t *)malloc(m_dataLen+1);
            if(m_dataBuf==nullptr)
            {
                m_dataBuf=nullptr;
                std::cout << "request dataBuf is null" << std::endl;
            }else
            {
                memset(m_dataBuf,'\0',m_dataLen+1);
            }
        }
        else
        {
            std::cout << "input dataLen is error" << std::endl;
        }
    }

    CameraPacket::~CameraPacket()
    {
        if (m_dataBuf != nullptr)
        {
            free(m_dataBuf);
            m_dataBuf = nullptr;
        }

        if(m_payloadBuf != nullptr)
        {
            free(m_payloadBuf);
            m_payloadBuf = nullptr;
        }
        m_haveData=false;
        m_haveAckData=false;
    }

    int32_t CameraPacket::setPayload(uint8_t *loadData, uint8_t loadLen)
    {
        int32_t ret=-1;
        if (loadData != nullptr && loadLen > 0)
        {
            m_payloadLen=loadLen;
            m_payloadBuf=(uint8_t *)malloc(m_payloadLen);
            if(m_payloadBuf==nullptr)
            {
                std::cout << "request payloadBuf is null" << std::endl;

            }else
            {
                memset(m_payloadBuf,0,m_payloadLen);
                memcpy(m_payloadBuf,loadData,m_payloadLen);
                m_haveData=true;
                ret=0;
            }

        }else
        {
            std::cout << "input dataBuf is null" << std::endl;
        }
        return ret;
    }

    int32_t CameraPacket::InitPacket(uint16_t header,uint8_t type,uint8_t flag,uint16_t srcId,uint16_t dstId,uint16_t msgId,uint8_t serial)
    {
        int32_t ret=-1;
        uint8_t *ptr = m_dataBuf;
        uint16_t tmp=0xffff;
        bool isLittle;
        
        if(!m_haveData || ptr==nullptr)
        {
           std::cout << "Load data not loaded" << std::endl;
           return ret;
        }

        if(m_dataLen < FRAME_FIXED_LEN+m_payloadLen)
        {
            std::cout << "dataLen is error" << std::endl;
            return ret;
        }

        try
        {
            m_header=header;
            m_type=type;
            m_flag=flag;
            m_srcId=srcId;
            m_dstId=dstId;
            m_msgId=msgId;
            m_serial=serial;

            isLittle=isLittleEndian();
            tmp = isLittle?m_header:toLittleEndian16(m_header);
            memcpy(ptr, &tmp, sizeof(uint16_t));
            ptr += 2;

            *ptr++ = m_type;
            *ptr++ = flag;

            tmp = isLittle?srcId:toLittleEndian16(srcId);
            memcpy(ptr, &tmp, sizeof(uint16_t));
            ptr += 2;

            tmp = isLittle?dstId:toLittleEndian16(dstId);
            memcpy(ptr, &tmp, sizeof(uint16_t));
            ptr += 2;

            tmp = isLittle?msgId:toLittleEndian16(msgId);
            memcpy(ptr, &tmp, sizeof(uint16_t));
            ptr += 2;

            *ptr++ = serial;
            *ptr++ = m_payloadLen;

            memcpy(ptr, m_payloadBuf, m_payloadLen);
            ptr += m_payloadLen;

            m_crcCheck=CRC16Update(m_crcCheck,m_dataBuf+2,FRAME_HEAD_LEN+m_payloadLen-2);
            m_crcCheck=isLittle ?m_crcCheck:toLittleEndian16(m_crcCheck);
            memcpy(ptr, &m_crcCheck, sizeof(uint16_t));
            ret=0;
        }
        catch(const std::exception& e)
        {
            std::cerr << e.what() << '\n';
        }
        return ret;
    }
    
    int32_t CameraPacket::setAckData(uint8_t *data, ssize_t len)
    {
        int32_t ret=-1;
        if(len >m_dataLen)
        {
            std::cout<< "input dataLen is error" << std::endl;
            return ret;
        }
        if (data != nullptr && len > 0)
        {
            memset(m_dataBuf,'\0',m_dataLen+1);
            memcpy(m_dataBuf,data,len);
            m_haveAckData=true;
            ret=0;
        }else
        {
            std::cout << "input dataBuf is null" << std::endl;
        }
        return ret;
    }

    // 解析数据
    bool CameraPacket::ParseData()
    {
        // 初始化crcSum为0xffff
        uint16_t crcSum=0xffff;
        // 初始化loadLen为0x00
        uint8_t loadLen=0x00;
        // 如果没有加载数据，输出提示信息并返回false
        if(!m_haveAckData)
        {
            std::cout << "data not loaded" << std::endl;
            return false;
        }
        // 获取数据长度
        loadLen=m_dataBuf[FRAME_HEAD_LEN-1];

        // 初始化m_crcCheck为0xffff
        m_crcCheck=0xffff;
        // 更新crcCheck
        m_crcCheck=CRC16Update(m_crcCheck,m_dataBuf+2,FRAME_HEAD_LEN+loadLen-2);
        
        // 获取crcSum
        crcSum=combineUint8ToUint16(m_dataBuf[FRAME_HEAD_LEN+loadLen+1],m_dataBuf[FRAME_HEAD_LEN+loadLen]);
        // 如果crcCheck不等于crcSum，输出提示信息并返回false
        if(m_crcCheck!=crcSum)
        {
            std::cout << "crc check error" << std::endl;
            return false;
        }

        if(m_dataBuf[0]!=0xfe || m_dataBuf[1]!=0x55)
        {
            std::cout << "data head is error" << std::endl;
            return false;
        }
        m_header=combineUint8ToUint16(m_dataBuf[1],m_dataBuf[0]);
        m_type=m_dataBuf[2];
        m_flag=m_dataBuf[3];
        m_srcId=combineUint8ToUint16(m_dataBuf[5],m_dataBuf[4]);
        m_dstId=combineUint8ToUint16(m_dataBuf[7],m_dataBuf[6]);
        m_msgId=combineUint8ToUint16(m_dataBuf[9],m_dataBuf[8]);
        m_serial=m_dataBuf[10];
        m_payloadLen=m_dataBuf[11];
        if(m_payloadBuf)
        {
            free(m_payloadBuf);
            m_payloadBuf=nullptr;
        }
        m_payloadBuf=(uint8_t *)malloc(m_payloadLen);

        if(m_payloadBuf==nullptr)
        {
            std::cout << "payloadBuf is null" << std::endl;
            return false;
        }
        memcpy(m_payloadBuf,m_dataBuf+FRAME_HEAD_LEN,m_payloadLen);

        return true;
    }

    uint8_t *CameraPacket::getDataBuf()
    {
        return m_dataBuf;
    }

    uint32_t CameraPacket::getDataLen()
    {
        return m_dataLen;
    }

    uint8_t CameraPacket::getType()
    {
        return m_type;
    }
    uint8_t CameraPacket::getFlag()
    {
        return m_flag;
    }
    uint16_t CameraPacket::getMsgId()
    {
        return m_msgId;
    }

    uint8_t *CameraPacket::getPayloadData()
    {
        return m_payloadBuf;
    }
    uint8_t CameraPacket::getPayloadLen()
    {
        return m_payloadLen;
    }

    uint16_t CameraPacket::combineUint8ToUint16(uint8_t highByte, uint8_t lowByte) 
    {
        return (static_cast<uint16_t>(highByte) << 8) | lowByte;
    }
    bool CameraPacket::isLittleEndian()
    {
        uint32_t num = 1;               // 0x00000001
        return (*(uint8_t *)&num == 1); // 小端：低字节为1；大端：高字节!=1
    }

    uint16_t CameraPacket::toLittleEndian16(uint16_t value)
    {
        return ((value >> 8) & 0xFF) | ((value & 0xFF) << 8); // 高低位互换
    }

    void CameraPacket::swapEndian(void *data, size_t size)
    {
        uint8_t *byte = (uint8_t *)data;
        for (size_t i = 0; i < size / 2; i++)
        {
            uint8_t temp = byte[i];
            byte[i] = byte[size - i - 1];
            byte[size - i - 1] = temp;
        }
    }

    uint16_t CameraPacket::CRC16Update(uint16_t crc, const uint8_t *data, size_t len)
    {
        while (len--) {
            crc ^= *data++;
            for (int i = 0; i < 8; i++) {
                if (crc & 1)
                    crc = (crc >> 1) ^ 0x8408; // 0x8408 是 0x1021 的位反转
                else
                    crc >>= 1;
            }
        }
        return crc;
    }

} // namespace gimabl